#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
//#include <AFMotor.h>
//#include <Servo.h> 

//Servo servo1;
//AF_DCMotor motor(4);

const char* ssid = "Xiaomi_5C2D";
const char* password = "82370288";

#define CHAR_PROTOCOLO_TECLADO '1'
#define CHAR_PROTOCOLO_TECLADO_2 '2'
#define PROTOCOLO_TECLADO 1
#define PROTOCOLO_TECLADO_2 2

/******************* INDICES DAS TECLAS ***************/
#define INDICE_PROTOCOLO 0

#define INDICE_NUMERO_CONTROLE "ISSO NAO E' MAIS USADO" //Deprecated! Duplicate INDICE_PROTOCOLO instead. I know...

#define INDICE_UP 1
#define INDICE_DOWN 2
#define INDICE_LEFT 3
#define INDICE_RIGHT 4

#define INDICE_QUADRADO 5
#define INDICE_BOLA 6
#define INDICE_XIS 7
#define INDICE_TRIANGULO 8

#define INDICE_L 9
#define INDICE_R 10
#define INDICE_START 11

#define INDICE_ANALOG_DOWN 12
#define INDICE_ANALOG_UP 13
#define INDICE_ANALOG_RIGHT 14
#define INDICE_ANALOG_LEFT 15

#define INDICE_SELECT 16

WiFiUDP Udp;
unsigned int localUdpPort = 30666;  // local port to listen on
char incomingPacket[255];  // buffer for incoming packets
char  replyPacket[] = "Hi there! Got the message :-)";  // a reply string to send back


void setup()
{
//  motor.setSpeed(255);
//  motor.run(RELEASE);
  
  Serial.begin(115200);
  Serial.println();

  Serial.printf("Connecting to %s ", ssid);
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }
  Serial.println(" connected");

  Udp.begin(localUdpPort);
  Serial.printf("Now listening at IP %s, UDP port %d\n", WiFi.localIP().toString().c_str(), localUdpPort);
}

//void faw(){
//   motor.run(FORWARD);
//}

//void back(){
//  motor.run(BACKWARD);
//}
//void stop(){
//  motor.run(RELEASE);
//}
//void right(){
//  for (int i=255; i!=0; i--) {
//    servo1.write(i-255);
//    delay(3);
//  }
//}
//void left(){
//  for (int i=0; i<=255; i--) {
//    servo1.write(i);
//    delay(3);
//  }
//}

void process(){
    if(incomingPacket[INDICE_PROTOCOLO] == CHAR_PROTOCOLO_TECLADO){
    if(incomingPacket[INDICE_UP] == '1')
    {
      Serial.println("INDICE_UP");
    }else if(incomingPacket[INDICE_DOWN] == '1')
      Serial.println("INDICE_DOWN");
   }
}
void loop()
{
  int packetSize = Udp.parsePacket();
  if (packetSize)
  {
    // receive incoming UDP packets
//    Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
    int len = Udp.read(incomingPacket, 17);
    if (len > 0)
    {
      incomingPacket[len] = 0;
    }
//    Serial.printf("UDP packet contents: %s\n", incomingPacket);
    process();
    // send back a reply, to the IP address and port we got the packet from
    Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
    Udp.write(replyPacket);
    Udp.endPacket();
  }
}
